//------------------------------------------------------------------------------ 
//  <copyright file="EstimatePlanarPose.cpp" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------

#include "Config.h" 
#include "HPTimer.h"
#include "Calibration.h"
#include "CheckerboardDetector.h"
#include "VisCalibration.h"
#include "EstimatePlanarPose.h"

using namespace std; 

/// <summary>
/// Initializes a new instance of the MSRPlanarPoseEstimator class.
/// </summary>
/// <param name="pdlColorIntrinsicParams">
///  Camera intrinsic parameters.
///  For more information refer to http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html
/// <param name="pdlColorIntrinsicParams[0]">
/// Color Intrinsic Parameter 1 for camera.
/// This is the Focal Length in x-direction (Fx) of the camera in pixels.
/// For more information refer to http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html
/// </param>
/// <param name="pdlColorIntrinsicParams[1]">Color Intrinsic Parameter 2 for camera. This is the Skew of the camera.</param>
/// <param name="pdlColorIntrinsicParams[2]">Color Intrinsic Parameter 3 for camera. This is the Focal Length in y-direction (Fy) of the camera in pixels.</param>
/// <param name="pdlColorIntrinsicParams[3]">Color Intrinsic Parameter 4 for camera. This is the position of the principle x-axis of the camera in pixels.</param>
/// <param name="pdlColorIntrinsicParams[4]">Color Intrinsic Parameter 5 for camera. This is the position of the principle y-axis of the camera in pixels.</param>
/// <param name="cbColorIntrinsicParams">Camera intrinsic parameters length, should be 5</param>
/// <param name="pdlColorRDistortionParams">  
///     Color Radial and Tangential Distortion Parameter 1 for camera.
///     For more information refer to http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html
/// </param>
/// <param name="cbColorIntrinsicParams">Camera radial and tangential distortion parameters length, should be 5</param>
MSRPlanarPoseEstimator::MSRPlanarPoseEstimator(
    double* pdlColorIntrinsicParams,
    int cbColorIntrinsicParams,
    double* pdlColorRDistortionParams,
    int cbColorRDistortionParams)
{
    if (cbColorIntrinsicParams != 5)
    {
        throw std::exception("cbColorIntrinsicParams should be 5");
    }

    if (cbColorRDistortionParams != 5)
    {
        throw std::exception("cbColorRDistortionParams should be 5");
    }

    m_colorIntrinsicParams = new CVisDVector();
    m_colorRDistortParams = new CVisDVector();
    m_colorIntrinsicParams->Resize(cbColorIntrinsicParams);
    m_colorRDistortParams->Resize(cbColorRDistortionParams);
    for (int i = 0; i < cbColorIntrinsicParams; ++i)
    {
       (*m_colorIntrinsicParams)[i] = pdlColorIntrinsicParams[i];
    }

    for (int i = 0; i < cbColorRDistortionParams; ++i)
    {
       (*m_colorRDistortParams)[i] = pdlColorRDistortionParams[i];
    }
}

/// <summary>
/// Destroys an instance of the MSRPlanarPoseEstimator class.
/// </summary>
MSRPlanarPoseEstimator::~MSRPlanarPoseEstimator()
{   
    SAFE_DELETE(m_colorIntrinsicParams);    
    SAFE_DELETE(m_colorRDistortParams);
}

/// <summary>
/// Estimate planar translation and rotation for the given image
/// Rotation is output as (Euler) angles.
/// </summary>
/// <param name="pbImageData">The image data in bytes in Gray 8 format </param>
/// <param name="dwImageLength">The image data length in bytes </param>
/// <param name="dwImageWidth">The image width in pixels</param>
/// <param name="dwImageHeight">The image height in pixels</param>
/// <param name="pdlTranslation">The translation in x, y, z directions of the planar checker board in mm</param>
/// <param name="cbTranslation">The length of translation buffer, should be 3</param>
/// <param name="pdlRotation">The rotation in x, y, z directions of the planar checker board in radians</param>
/// <param name="cbRotation">The length of rotation array, should be 3</param>
/// <returns>True if the estimation was successful, False otherwise</returns>
bool MSRPlanarPoseEstimator::EsimatePlanarPose(
    BYTE *pbImageData,
    DWORD dwImageLength,
    DWORD dwImageWidth,
    DWORD dwImageHeight,
    double* pdlTranslation, 
    DWORD cbTranslation,
    double* pdlRotation,
    DWORD cbpdlRotation)
{
    if (cbTranslation != 3)
    {
        throw std::exception("cbTranslation should be 3");
    }

    if (cbpdlRotation != 3)
    {
        throw std::exception("cbpdlRotation should be 3");
    }

    // Estimate pose with rotation represented as quaternion
    double quaternion[4];
    bool success = this->EsimatePlanarPoseWithRotationAsUnitQuaternion(
        pbImageData,
        dwImageLength,
        dwImageWidth,
        dwImageHeight,
        pdlTranslation, 
        cbTranslation,
        quaternion,
        _ARRAYSIZE(quaternion));
    if (!success)
    {
        return false;
    }

    // Convert quaternion to angles
    return this->QuaternionToEulerAngles(quaternion, _ARRAYSIZE(quaternion), pdlRotation, cbpdlRotation);
}

/// <summary>
/// Estimate planar translation and rotation for the given image.
/// Rotation is output as a unit quaternion.
/// </summary>
/// <param name="pbImageData">The image data in bytes in Gray 8 format </param>
/// <param name="dwImageLength">The image data length in bytes </param>
/// <param name="dwImageWidth">The image width in pixels</param>
/// <param name="dwImageHeight">The image height in pixels</param>
/// <param name="pdlTranslation">The translation in x, y, z directions of the planar checker board in mm</param>
/// <param name="cbTranslation">The length of translation buffer, should be 3</param>
/// <param name="pdlQuaternion">The rotation of the planar checker board, specified as a unit quaternion</param>
/// <param name="cbQuaternion">The length of rotation array, should be 4</param>
/// <returns>True if the estimation was successful, False otherwise</returns>
bool MSRPlanarPoseEstimator::EsimatePlanarPoseWithRotationAsUnitQuaternion(
    BYTE *pbImageData,
    DWORD dwImageLength,
    DWORD dwImageWidth,
    DWORD dwImageHeight,
    double* pdlTranslation, 
    DWORD cbTranslation,
    double* pdlQuaternion,
    DWORD cbQuaternion)
{
    CCheckerboardDetector checkerboard_detector; 

    if (cbTranslation != 3)
    {
        throw std::exception("cbTranslation should be 3");
    }

    if (cbQuaternion != 4)
    {
        throw std::exception("cbpdlQuaternion should be 4");
    }

    if (!checkerboard_detector.LoadCheckerBoardInfo("Data\\GroundTruthExtraction\\calib_board.txt"))
    {
        throw std::exception("Unable to load checkerboard file Data\\GroundTruthExtraction\\calib_board.txt.\n");
    }

    if (!checkerboard_detector.m_bInitialized)
    {
        return false;
    }

    vector<CVisDMatrix> seqPtsModel; 
    vector<CVisDMatrix> seqPtsImage;

    // Min and max landmark sizes
    int nMinSize = 50; 
    int nMaxSize = 10000; 
    
    CImageG img(dwImageWidth, dwImageHeight, dwImageWidth, pbImageData);   
    
    // If we did not detect the checker board, bail out.
    if (!checkerboard_detector.Detect(&img, NULL, nMinSize, nMaxSize))
    {
        return false;
    }
    
    // Add the model points and detected points to feed them to the camera plane estimation function.
    float flSize = checkerboard_detector.m_CBInfo.m_fSize; 
    CheckerBoardPose* pose = &checkerboard_detector.m_CurrCBPose;

    if (pose == NULL)  
    {
        return false;
    }

    int num = pose->GetNumFound(); 

    /* Send same image points 3 times. 
    The MSR code does outlier rejection only if there are three images.
    This ensures that outlier rejection is performed on the images. 
    At the higher level, we ensure that three consecutive poses are within 1mm,
    which is more accurate than sending 3 images to MSR and returning the average estimate of those three images, especially if the camera is moving. */

    int imageCopyCount = 3;

    seqPtsModel.resize(imageCopyCount);
    seqPtsImage.resize(imageCopyCount);
    for (int i = 0; i < imageCopyCount; ++i)
    {
        seqPtsModel[i].Resize(num, 2);
        seqPtsImage[i].Resize(num, 2);
        int nIndex = 0; 
        for (int n = 0; n < pose->m_nSize; n++) 
        {
            if (pose->m_pbFound[n]) 
            {
                seqPtsModel[i][nIndex][0] = flSize * checkerboard_detector.m_CBInfo.m_pModelPts[n].x; 
                seqPtsModel[i][nIndex][1] = flSize  *checkerboard_detector.m_CBInfo.m_pModelPts[n].y; 
                seqPtsImage[i][nIndex][0] = pose->m_pImagePts[n].x; 
                seqPtsImage[i][nIndex][1] = pose->m_pImagePts[n].y; 
                nIndex++; 
            }
        }
    }

    int rgnResolution[] = {dwImageWidth, dwImageHeight};
    CVisCalib1Cam mvCalib(seqPtsModel, seqPtsImage, rgnResolution);

    // Set camera parameters
    mvCalib.SetIntrinsicParams(*m_colorIntrinsicParams); 
    mvCalib.SetDistortParams(*m_colorRDistortParams); 

    // Estimate planes tunable parameters
    const double c_dlTol = 1.0e-02;
    const int c_nMaxIters = 100;
    const bool c_fRemoveOutliers = true;


    // Estimate plane. The results are stored in mvCalib.Rotations and mvCalib.Translations

    bool valid = mvCalib.EstimatePlanes(c_fRemoveOutliers, c_dlTol, c_nMaxIters);
    if (valid)
    {
        const CVisRotation* pRotations = mvCalib.Rotations();
        const CVisDVector* pTranslations = mvCalib.Translations();
        if (pRotations != NULL && pRotations->Vector().Length() == 3 && pTranslations != NULL && pTranslations[0].Length() == 3)
        {
            const CVisDVector* pRotationVector  = &pRotations->Quaternion();
            const CVisDVector* pTranslationVector = &pTranslations[0];         

            // Normalize quaternion to yield unit quaternion
            pdlQuaternion[0] = (*pRotationVector)[0];
            pdlQuaternion[1] = (*pRotationVector)[1];
            pdlQuaternion[2] = (*pRotationVector)[2];
            pdlQuaternion[3] = (*pRotationVector)[3];
            bool success = this->NormalizeQuaternion(pdlQuaternion, cbQuaternion);
            if (!success)
            {
                return false;
            }

            // The MSR Planar Estimator outputs in millimeters, convert it to meters.
            const double millimetersToMeters = 1.0 / 1000;
            pdlTranslation[0] = (*pTranslationVector)[0] * millimetersToMeters;
            pdlTranslation[1] = (*pTranslationVector)[1] * millimetersToMeters;
            pdlTranslation[2] = (*pTranslationVector)[2] * millimetersToMeters;
         
        }
    }

    seqPtsModel.clear();
    seqPtsImage.clear();
    return valid;
}

/// <summary>
/// Convert quaternion to euler angles
/// </summary>
/// <param name="pdlQuaternion">The quaternion which represents the rotation of the planar checker board</param>
/// <param name="cbQuaternion">The length of quaternion array, should be 4</param>    
/// <param name="pdlRotation">The euler angles in x, y, z directions of the planar checker board in radians</param>
/// <param name="cbRotation">The length of rotation array, should be 3</param>    
/// <returns> True if the conversion was successful, False otherwise </returns>
bool MSRPlanarPoseEstimator::QuaternionToEulerAngles(double *pdlQuaternion, DWORD cbQuaternion, double *pdlRotation, DWORD cbRotation)
{       
        bool success = this->NormalizeQuaternion(pdlQuaternion, cbQuaternion);
        if (!success)
        {
            return false;
        }

        double q0 =  pdlQuaternion[0];
        double q1 =  pdlQuaternion[1];
        double q2 =  pdlQuaternion[2];
        double q3  =  pdlQuaternion[3];

        // quaternion to euler angles
        pdlRotation[0] = atan2(2 * q0 *q1 + q2 * q3, (1-2 * (q1 * q1 + q2 * q2)));
        pdlRotation[1] = asin(2 * (q0 * q2 - q3 * q1));
        pdlRotation[2] = atan2(2 * q0 * q3 + q1 * q2, (1-2 * (q2 * q2 + q3 * q3)));

        return true;
}

/// <summary>
/// Normalize quaternion by dividing all 4 values by the L2 norm of the vector, resulting in a unit quaternion.
/// </summary>
/// <param name="pdlQuaternion">The quaternion which represents the rotation of the planar checker board</param>
/// <param name="cbQuaternion">The length of quaternion array, should be 4</param>    
/// <returns> True if the conversion was successful, False otherwise </returns>
bool MSRPlanarPoseEstimator::NormalizeQuaternion(double *pdlQuaternion, DWORD cbQuaternion)
{
        double q0 =  pdlQuaternion[0];
        double q1 =  pdlQuaternion[1];
        double q2 =  pdlQuaternion[2];
        double q3 =  pdlQuaternion[3];
        double s = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
            
        // Avoid divide by zero to safeguard against any bugs in MSRA codebase.
        if (s <= 0)
        {
            return false;
        }

        // Normalize the quaternion
        pdlQuaternion[0] = q0 / s;
        pdlQuaternion[1] = q1 / s;
        pdlQuaternion[2] = q2 / s;
        pdlQuaternion[3] = q3 / s;

        return true;
}
